#ifndef D435_HPP
#define D435_HPP
#include "sr300.hpp"
#include "findmarker.hpp"

class D435 : public SR300, public FindMarker
{
public:
    D435()
        : SR300()
        , FindMarker()
    {

    }


    static cv::Vec3f SubPixel(const cv::Mat &xyz, cv::Point2f pt)
    {
        if(!pt.inside(cv::Rect2f(0,0,xyz.cols-1,xyz.rows-1)))
            return cv::Vec3f(0,0,0);

        cv::Point pto(floor(pt.x),floor(pt.y));

        cv::Mat roi=xyz(cv::Rect(pto,cv::Size(2,2)));

        for(size_t i=0;i<roi.total();i++)
        {
            if(roi.at<cv::Vec3f>(i)==cv::Vec3f(0,0,0))
                return cv::Vec3f(0,0,0);
        }

        cv::Mat patch;
        cv::getRectSubPix(xyz, cv::Size(1,1), pt, patch);

        return patch.at<cv::Vec3f>(0);
    }




    virtual void ProcessObject(int id, const std::vector<cv::Vec3f> &xyz)
    {        
        std::cout<<"id="<<id<<"\nxyz="<<cv::Mat(xyz)<<"\n"<<std::flush;
        std::ostringstream oss;
        oss<<id<<".txt";
        std::ofstream ofs(oss.str());
        if(ofs)
        {
            ofs<<cv::Mat(xyz);
            ofs.close();
        }
    }





    virtual void ProcessObject2(int id, cv::Point2f pt, const cv::Mat &aligned_xyz)
    {
         std::cout<<"id="<<id<<"\npt="<<pt<<"\n"<<std::flush;

    }



    virtual void ProcessFrame(const cv::Mat &color, const cv::Mat &ir, const cv::Mat &depth, const cv::Mat &xyz, const cv::Mat &texture, const cv::Mat &aligned_depth, const cv::Mat &aligned_xyz)
    {

        cv::Mat gray;
        cv::cvtColor(color, gray, CV_BGR2GRAY);

        cv::aruco::detectMarkers(gray, dictionary, corners, ids, detectorParams, rejected);

        for(size_t i=0;i<ids.size();i++)
        {
#if 1
          if(corners[i].size()==4)
              ProcessObject2(ids[i], Center(corners[i]), aligned_xyz);
#else
            std::vector<cv::Vec3f> corner_xyz;
            for(auto &pt:corners[i])
            {
                cv::Vec3f sp=SubPixel(aligned_xyz, pt);
                if(sp[2]!=0)
                    corner_xyz.push_back(sp);
            }
            if(corner_xyz.size()==4)
                ProcessObject(ids[i],corner_xyz);
#endif
        }

        cv::Mat color2=color;
        FindMarker::DrawResult(color2);




        cv::imwrite("depth.png", ColorMap(aligned_depth));

        //       SR300::ProcessFrame(color, ir, depth, xyz, texture, aligned_depth, aligned_xyz);

        cv::imwrite("color.png", color);

        cv::imwrite("axyz.hdr",aligned_xyz);

        // Display in a GUI
        cv::namedWindow("color Image", cv::WINDOW_AUTOSIZE );
        cv::imshow("color Image", color);
        cv::namedWindow("aligned depth Image ColorMap", cv::WINDOW_AUTOSIZE );
        cv::imshow("aligned depth Image ColorMap", ColorMap(aligned_depth));
        k=cv::waitKey(1);


    }
};

#endif

